﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.ComponentModel;
using Nui.Vision;
using Nui;
using xn;

namespace KinectBodyTracking
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        NuiUserTracker _skeleton;
        readonly int IMAGE_WIDTH;
        readonly int IMAGE_HEIGHT;

        Nui.NuiHandTracker _handTracker;

        bool _isPainting = true;
        //Context _context;
        BackgroundWorker _worker = new BackgroundWorker();
        Color[] _userColors = { Colors.Blue, Colors.Red, Colors.Green, Colors.Yellow, Colors.Pink, Colors.Black };
        int node;

        public MainWindow()
        {
            InitializeComponent();
            CompositionTarget.Rendering += new EventHandler(CompositionTarget_Rendering);

            _skeleton = new NuiUserTracker("SamplesConfig.xml");
            _skeleton.UserUpdated += new NuiUserTracker.UserUpdatedHandler(Skeleton_UserUpdated);

            _handTracker = new Nui.NuiHandTracker("SamplesConfig.xml");
            _handTracker.Hover += new Nui.NuiHandTracker.HoverHandler(Hands_Hover);
            _handTracker.Push += new Nui.NuiHandTracker.PushHandler(Hands_Push);
            //_context = new Context("SamplesConfig.xml");
            //_context.OpenFileRecording("D:\\Master\\Semestre2\\TER\\Installation\\KinectBodyTracking\\KinectBodyTracking\\KinectBodyTracking\\SkeletonRec.oni");

            IMAGE_WIDTH = _handTracker.Width;
            IMAGE_HEIGHT = _handTracker.Height;

            _worker.DoWork += new DoWorkEventHandler(Worker_DoWork);
        }

        void Worker_DoWork(object sender, DoWorkEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                imgCamera1.Source = _skeleton.RawImage;
                imgCamera2.Source = _skeleton.DepthImage;
            });
        }

        void CompositionTarget_Rendering(object sender, EventArgs e)
        {
            if (!_worker.IsBusy)
            {
                _worker.RunWorkerAsync();
            }
        }

        void Skeleton_UserUpdated(object sender, NuiUserEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                LayoutRoot.Children.Clear();
                foreach (var user in e.Users)
                {
                    Color userColor = _userColors[user.Id - 1];

                    #region Get body parts coordinates

                    float headX = user.Head.X;
                    float headY = user.Head.Y;
                    float headZ = user.Head.Z;

                    float neckX = user.Neck.X;
                    float neckY = user.Neck.Y;
                    float neckZ = user.Neck.Z;

                    float leftShoulderX = user.LeftShoulder.X;
                    float leftShoulderY = user.LeftShoulder.Y;
                    float leftShoulderZ = user.LeftShoulder.Z;

                    float leftElbowX = user.LeftElbow.X;
                    float leftElbowY = user.LeftElbow.Y;
                    float leftElbowZ = user.LeftElbow.Z;

                    float leftHandX = user.LeftHand.X;
                    float leftHandY = user.LeftHand.Y;
                    float leftHandZ = user.LeftHand.Z;

                    float rightShoulderX = user.RightShoulder.X;
                    float rightShoulderY = user.RightShoulder.Y;
                    float rightShoulderZ = user.RightShoulder.Z;

                    float rightElbowX = user.RightElbow.X;
                    float rightElbowY = user.RightElbow.Y;
                    float rightElbowZ = user.RightElbow.Z;

                    float rightHandX = user.RightHand.X;
                    float rightHandY = user.RightHand.Y;
                    float rightHandZ = user.RightHand.Z;

                    float waistX = user.Torso.X;
                    float waistY = user.Torso.Y;
                    float waistZ = user.Torso.Z;

                    float leftHipX = user.LeftHip.X;
                    float leftHipY = user.LeftHip.Y;
                    float leftHipZ = user.LeftHip.Z;

                    float leftKneeX = user.LeftKnee.X;
                    float leftKneeY = user.LeftKnee.Y;
                    float leftKneeZ = user.LeftKnee.Z;

                    float leftFootX = user.LeftFoot.X;
                    float leftFootY = user.LeftFoot.Y;
                    float leftFootZ = user.LeftFoot.Z;

                    float rightHipX = user.RightHip.X;
                    float rightHipY = user.RightHip.Y;
                    float rightHipZ = user.RightHip.Z;

                    float rightKneeX = user.RightKnee.X;
                    float rightKneeY = user.RightKnee.Y;
                    float rightKneeZ = user.RightKnee.Z;

                    float rightFootX = user.RightFoot.X;
                    float rightFootY = user.RightFoot.Y;
                    float rightFootZ = user.RightFoot.Z;

                    #endregion

                    #region Print selected node x, y, z
                    
                    textBoxX.Clear();
                    textBoxY.Clear();
                    textBoxZ.Clear();
                    switch (node)
                    {
                        case 0:
                            textBoxX.AppendText(headX.ToString());
                            textBoxY.AppendText(headY.ToString());
                            textBoxZ.AppendText(headZ.ToString());
                            break;
                        case 1:
                            textBoxX.AppendText(neckX.ToString());
                            textBoxY.AppendText(neckY.ToString());
                            textBoxZ.AppendText(neckZ.ToString());
                            break;
                        case 2:
                            textBoxX.AppendText(leftShoulderX.ToString());
                            textBoxY.AppendText(leftShoulderY.ToString());
                            textBoxZ.AppendText(leftShoulderZ.ToString());
                            break;
                        case 3:
                            textBoxX.AppendText(leftElbowX.ToString());
                            textBoxY.AppendText(leftElbowY.ToString());
                            textBoxZ.AppendText(leftElbowZ.ToString());
                            break;
                        case 4:
                            textBoxX.AppendText(leftHandX.ToString());
                            textBoxY.AppendText(leftHandY.ToString());
                            textBoxZ.AppendText(leftHandZ.ToString());
                            break;
                        case 5:
                            textBoxX.AppendText(rightShoulderX.ToString());
                            textBoxY.AppendText(rightShoulderY.ToString());
                            textBoxZ.AppendText(rightShoulderZ.ToString());
                            break;
                        case 6:
                            textBoxX.AppendText(rightElbowX.ToString());
                            textBoxY.AppendText(rightElbowY.ToString());
                            textBoxZ.AppendText(rightElbowZ.ToString());
                            break;
                        case 7:
                            textBoxX.AppendText(rightHandX.ToString());
                            textBoxY.AppendText(rightHandY.ToString());
                            textBoxZ.AppendText(rightHandZ.ToString());
                            break;
                        case 8:
                            textBoxX.AppendText(waistX.ToString());
                            textBoxY.AppendText(waistY.ToString());
                            textBoxZ.AppendText(waistZ.ToString());
                            break;
                        case 9:
                            textBoxX.AppendText(leftKneeX.ToString());
                            textBoxY.AppendText(leftKneeY.ToString());
                            textBoxZ.AppendText(leftKneeZ.ToString());
                            break;
                        case 10:
                            textBoxX.AppendText(leftHipX.ToString());
                            textBoxY.AppendText(leftHipY.ToString());
                            textBoxZ.AppendText(leftHipZ.ToString());
                            break;
                        case 11:
                            textBoxX.AppendText(leftFootX.ToString());
                            textBoxY.AppendText(leftFootY.ToString());
                            textBoxZ.AppendText(leftFootZ.ToString());
                            break;
                        case 12:
                            textBoxX.AppendText(rightKneeX.ToString());
                            textBoxY.AppendText(rightKneeY.ToString());
                            textBoxZ.AppendText(rightKneeZ.ToString());
                            break;
                        case 13:
                            textBoxX.AppendText(rightHipX.ToString());
                            textBoxY.AppendText(rightHipY.ToString());
                            textBoxZ.AppendText(rightHipZ.ToString());
                            break;
                        case 14:
                            textBoxX.AppendText(rightFootX.ToString());
                            textBoxY.AppendText(rightFootY.ToString());
                            textBoxZ.AppendText(rightFootZ.ToString());
                            break;
                        default :
                            break;
                    }

                    #endregion

                    #region Draw skeleton points

                    DrawCircle(headX, headY, headZ, userColor);
                    DrawCircle(neckX, neckY, neckZ, userColor);
                    DrawCircle(leftShoulderX, leftShoulderY, leftShoulderZ, userColor);
                    DrawCircle(leftElbowX, leftElbowY, leftElbowZ, userColor);
                    if (leftHandY < headY)
                    {
                        DrawCircle(leftHandX, leftHandY, leftHandZ, Colors.Chocolate);
                    }
                    else
                    {
                        DrawCircle(leftHandX, leftHandY, leftHandZ, userColor);
                    }
                    DrawCircle(waistX, waistY, waistZ, userColor);
                    DrawCircle(rightShoulderX, rightShoulderY, rightShoulderZ, userColor);
                    DrawCircle(rightElbowX, rightElbowY, rightElbowZ, userColor);
                    if (rightHandY < headY)
                    {
                        DrawCircle(rightHandX, rightHandY, rightHandZ, Colors.Chocolate);
                    }
                    else
                    {
                        DrawCircle(rightHandX, rightHandY, rightHandZ, userColor);
                    }
                    DrawCircle(leftHipX, leftHipY, leftHipZ, userColor);
                    DrawCircle(leftKneeX, leftKneeY, leftKneeZ, userColor);
                    DrawCircle(leftFootX, leftFootY, leftFootZ, userColor);
                    DrawCircle(rightHipX, rightHipY, rightHipZ, userColor);
                    DrawCircle(rightKneeX, rightKneeY, rightKneeZ, userColor);
                    DrawCircle(rightFootX, rightFootY, rightFootZ, userColor);

                    #endregion

                    #region Draw skeleton line

                    DrawLine(headX, headY, neckX, neckY, userColor);
                    DrawLine(neckX, neckY, leftShoulderX, leftShoulderY, userColor);
                    DrawLine(neckX, neckY, rightShoulderX, rightShoulderY, userColor);
                    DrawLine(leftShoulderX, leftShoulderY, leftElbowX, leftElbowY, userColor);
                    DrawLine(leftElbowX, leftElbowY, leftHandX, leftHandY, userColor);
                    DrawLine(neckX, neckY, waistX, waistY, userColor);
                    DrawLine(rightShoulderX, rightShoulderY, rightElbowX, rightElbowY, userColor);
                    DrawLine(rightElbowX, rightElbowY, rightHandX, rightHandY, userColor);
                    DrawLine(waistX, waistY, leftHipX, leftHipY, userColor);
                    DrawLine(waistX, waistY, rightHipX, rightHipY, userColor);
                    DrawLine(leftHipX, leftHipY, leftKneeX, leftKneeY, userColor);
                    DrawLine(leftKneeX, leftKneeY, leftFootX, leftFootY, userColor);
                    DrawLine(rightHipX, rightHipY, rightKneeX, rightKneeY, userColor);
                    DrawLine(rightKneeX, rightKneeY, rightFootX, rightFootY, userColor);

                    #endregion

                }
            });
        }

        void Hands_Hover(object sender, Nui.NuiPositionEventArgs e)
        {
            // Hover event.
            if (_isPainting)
            {
                // Coordinates range from -320 to 320 and -240 to 240.
                DrawPixels(e.X + (IMAGE_WIDTH / 2), e.Y + (IMAGE_HEIGHT / 2));
            }
        }

        void Hands_Push(object sender, Nui.NuiPositionEventArgs e)
        {
            // Push event!
            _isPainting = !_isPainting;
        }

        private void DrawPixels(float x, float y)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                // We'll use random colors!
                Random rand = new Random();

                Ellipse ellipse = new Ellipse
                {
                    Fill = new SolidColorBrush(Color.FromRgb((byte)rand.Next(0, 255), (byte)rand.Next(0, 255), (byte)rand.Next(0, 255))),
                    Width = 25,
                    Height = 25
                };

                Canvas.SetLeft(ellipse, x);
                Canvas.SetBottom(ellipse, y);

                LayoutRoot.Children.Add(ellipse);
            });
        }

        void DrawCircle(float positionX, float positionY, float positionZ, Color color)
        {
            float diametre = 50 - ((positionZ / 100) * 2);
            
            Ellipse ellipse = new Ellipse
            {
                Fill = new SolidColorBrush(color),
                Width = diametre,
                Height = diametre,
                Margin = new Thickness(positionX, positionY, 0, 0)
            };
            LayoutRoot.Children.Add(ellipse);
        }

        void DrawLine(float positionX1, float positionY1, float positionX2, float positionY2, Color color)
        {
            Line line = new Line
            {
                X1 = positionX1,
                Y1 = positionY1,
                X2 = positionX2,
                Y2 = positionY2,
                Stroke = new SolidColorBrush(color),
                StrokeThickness= 4 
            };
            LayoutRoot.Children.Add(line);
        }

        private void comboBox1_SelectionChanged(object sender, SelectionChangedEventArgs e)
        {
            node = comboBox1.SelectedIndex;
        }
    }
}
